#!/usr/bin/python
#-*- coding:UTF-8 -*-

import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point
import threading

class Sender():
    
    def __init__(self) :
        # rospy.init_node('point_visualization')

        # 创建一个发布器发布Marker消息
        self.marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
        
        self.marker = None

        tasksent = threading.Thread(target=self._sent)
        tasksent.start()

    def sent(self,point2send):
        x,y,z = point2send

        marker = Marker()
        # marker.header.frame_id = "arm_camera_link" # 设置相机坐标系的名称
        marker.header.frame_id = "panda_link0" # 设置相机坐标系的名称
        marker.header.stamp = rospy.Time.now()
        marker.ns = "points"
        marker.id = 0
        marker.type = Marker.POINTS
        marker.action = Marker.ADD
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.color.a = 1.0 # 不透明
        marker.color.r = 1.0
        marker.color.g = 0.0
        marker.color.b = 0.0
        
        

        point = Point()
        point.x = x
        point.y = y
        point.z = z
        marker.points.append(point)

        self.marker = marker

    def _sent(self):
        rate = rospy.Rate(30) 

        while not rospy.is_shutdown():
            if self.marker is not None:
                self.marker.header.stamp = rospy.Time.now()
                self.marker_pub.publish(self.marker)
            rate.sleep()

if __name__ == '__main__':
    rospy.init_node('point_visualization')
    a = Sender()
    c = 0
    
    while 1:
        
        a.sent([0,0,2])
    

   

    